#!/usr/bin/env python
# coding=utf-8

import rospy
from std_msgs.msg import Int8
from sensor_msgs.msg import Image
from std_msgs.msg import String
import cv2, cv_bridge
import numpy
import os
import sys
from tarkbot_driver_yolo.msg import ControlWay
import threading
import time
from geometry_msgs.msg import Twist
from tarkbot_yolov5.msg import *

from dynamic_reconfigure.server import Server
from tarkbot_driver_yolo.cfg import paramsConfig

ring_image_x_left = 0
ring_image_x_right = 0.6
ring_image_y_top = 0.6
ring_image_y_bot = 0.85
ring_center_target = 0.20
ring_vel_x = 0.15
ring_vel_y_P = 0
ring_vel_y_D = 0
ring_vel_z_P = 0.01
ring_vel_z_D = 0.001

start_flag = 0
go_outside_flag = 0
wait_flag = 0
slow_flag = 0

driver_mode_pub = rospy.Publisher("/driver_mode", ControlWay, queue_size=1)


def thread_job():
    rospy.spin()


def start_flag_callback(msg):
    global start_flag
    start_flag = msg.data


##########################################################################################################################
#                                                                                                                        #
#                                               图像识别回调函数编写部分     
#                                                                                                                        #
##########################################################################################################################

# 图像识别回调
def side_flag_callback(msg):
    global wait_flag
    global slow_flag

    if msg.data == []:
        pass
    elif msg.data[0].frame_id == "Sidewalk":
        if msg.data[0].ptx > 450 and msg.data[0].disth > 50:
            print("Sidewalk")
            wait_flag = 1
    elif msg.data[0].frame_id == "slow":
        if msg.data[0].ptx > 450 and msg.data[0].disth > 50:
            print("slow")
            slow_flag = 1

def ctrl_data(tarkbot_driver_way):
    global ring_image_x_left
    global ring_image_x_right
    global ring_image_y_top
    global ring_image_y_bot
    global ring_center_target
    global ring_vel_x
    global ring_vel_y_P
    global ring_vel_y_D
    global ring_vel_z_P
    global ring_vel_z_D

    drivermode = ControlWay()

    if tarkbot_driver_way == 1:
        drivermode.mask_x_left = ring_image_x_left
        drivermode.mask_x_right = ring_image_x_right
        drivermode.mask_y_top = ring_image_y_top
        drivermode.mask_y_bot = ring_image_y_bot
        drivermode.center_target = ring_center_target
        drivermode.vel_x = ring_vel_x
        drivermode.vel_y_P = ring_vel_y_P
        drivermode.vel_y_D = ring_vel_y_D
        drivermode.vel_z_P = ring_vel_z_P
        drivermode.vel_z_D = ring_vel_z_D
        drivermode.en = 1

    elif tarkbot_driver_way == 2:
        drivermode.mask_x_left = ring_image_x_left
        drivermode.mask_x_right = ring_image_x_right
        drivermode.mask_y_top = ring_image_y_top
        drivermode.mask_y_bot = ring_image_y_bot
        drivermode.center_target = ring_center_target
        drivermode.vel_x = ring_vel_x
        drivermode.vel_y_P = ring_vel_y_P
        drivermode.vel_y_D = ring_vel_y_D
        drivermode.vel_z_P = ring_vel_z_P
        drivermode.vel_z_D = ring_vel_z_D
        drivermode.en = 4
    return drivermode


def reconfigCB(config, level):
    global ring_center_target
    global ring_vel_z_P
    global ring_vel_z_D

    ring_center_target = config.ring_center_target
    ring_vel_z_P = config.ring_vel_z_P
    ring_vel_z_D = config.ring_vel_z_D

    return config


def control_drive():
    tarkbot_driver_way = 0
    global start_flag
    global go_outside_flag
    global wait_flag
    global slow_flag
    global ring_center_target
    global ring_vel_z_P
    global ring_vel_z_D

    drivermode_pub = ControlWay()
    msg = Int8()

    rospy.init_node("control_drive")

    add_thread = threading.Thread(target=thread_job)
    add_thread.start

    cmdvel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
    side_flag_sub = rospy.Subscriber("Yolov5object", ObjectArray, side_flag_callback)
    start_flag_sub = rospy.Subscriber("/start_flag", Int8, start_flag_callback)
    dynamic_reconfigure_server = Server(paramsConfig, reconfigCB)
    ring_center_target = rospy.get_param('~ring_center_target', 0.26)
    ring_vel_z_P = rospy.get_param('~ring_vel_z_P', 0.01)
    ring_vel_z_D = rospy.get_param('~ring_vel_z_D', 0.001)

    rate = rospy.Rate(100)

    ##########################################################################################################################
    #                                                                                                                        #
    #                                               逻辑代码编写部分
    #                                                                                                                        #
    ##########################################################################################################################

    while not rospy.is_shutdown():

        if start_flag == 1:
            if tarkbot_driver_way == 0:
                tarkbot_driver_way = 1
                drivermode_pub = ctrl_data(tarkbot_driver_way)
                driver_mode_pub.publish(drivermode_pub)

            elif tarkbot_driver_way == 1:
                if slow_flag == 1:
                    car_stop = ControlWay()
                    driver_mode_pub.publish(car_stop)
                    carmove = Twist()
                    carmove.linear.x = 0.07 # v
                    carmove.linear.y = 0
                    carmove.angular.z = 0
                    cmdvel_pub.publish(carmove)
                    time.sleep(5)
                    drivermode_pub = ctrl_data(tarkbot_driver_way)
                    driver_mode_pub.publish(drivermode_pub)
                    slow_flag = 0
                
                if wait_flag == 1:
                    car_stop = ControlWay()
                    driver_mode_pub.publish(car_stop)
                    carmove = Twist()
                    carmove.linear.x = 0
                    carmove.linear.y = 0
                    cmdvel_pub.publish(carmove)
                    time.sleep(5)

                    carmove = Twist()
                    carmove.linear.x = 0.15
                    carmove.linear.y = 0
                    carmove.angular.z = 0
                    cmdvel_pub.publish(carmove)
                    time.sleep(2.5)

                    drivermode_pub = ctrl_data(tarkbot_driver_way)
                    driver_mode_pub.publish(drivermode_pub)
                    wait_flag = 0
                    

                    



        else:
            rospy.loginfo("Robot waiting for init ...")
        rate.sleep()


if __name__ == '__main__':
    try:
        control_drive()
    except rospy.ROSInterruptException:
        pass
